#include "PeopleDetector.h"

using namespace std;
using namespace cv;

PeopleDetector::PeopleDetector(){
        max_ratio = 7.4;
        min_ratio = 0.8;
}

PeopleDetector::PeopleDetector(float min, float max){
    max_ratio = max;
    min_ratio = min;
}

/** @brief Find people in image
*   @param Mat &mat, Mat &mask
*   @return vector<Person>
*
* Finds people in image, creates Person object for each
* and return vector of found people.
*
*/
vector<Person> PeopleDetector::findPeople(Mat &mat, Mat &mask, Mat &depth){
    CBlobResult blobs;
    vector<Person> result;

    //Create iplimages from matrices
	IplImage ipl = mat;
	IplImage *ipoint;
	ipoint = &ipl;

	IplImage mask_ipl = mask;
	IplImage *mpoint;
	mpoint = &mask_ipl;

	// Extract the blobs using a threshold of 150 in the image
	blobs = CBlobResult(ipoint, mpoint, 220, true );


	// Discard the blobs with less area than 20000 pixels
	blobs.Filter(blobs, B_INCLUDE, CBlobGetArea(), B_GREATER, 20000);
	blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER, 120000 );


    //Check if blobs are people and add to result if they are
    CBlob *currentBlob;
    for (int i = 0; i < blobs.GetNumBlobs(); i++ ){
        currentBlob = blobs.GetBlob(i);
        if(isPerson(currentBlob)){
            Person p = createPerson(currentBlob, i, depth);
            result.push_back(p);

            Rect c;
            c.x = currentBlob->minx;
            c.width = currentBlob->maxx - currentBlob->minx;
            c.y = currentBlob->miny;
            c.height = currentBlob->maxy -currentBlob->miny;

            rectangle(mat, c.tl(), c.br(), cv::Scalar(255,0,0), 3);
        }
    }

    return result;
}

/** @brief Determines if blob is person
*   @param CBlob *blob
*   @return bool
*
* Uses height/width ratio to determine if blob
* is a person.
*
*/
bool PeopleDetector::isPerson(CBlob *blob){
    double ratio, height, width;

    height = (blob->maxy - blob->miny);
    width = (blob->maxx - blob->minx);

    ratio = height / width;

    //Blobs with this ratio are likely to be people
    if (ratio > min_ratio && ratio < max_ratio){
        return true;
    }

    return false;

}

/** @brief Creates Person using blob information
*   @param CBlob *blob, int id
*   @return Person
*
* Creates Person using blob information
*
*/
Person PeopleDetector::createPerson(CBlob *blob, int id, Mat &depth){
    Person p(id);

    //Set person properties

    int centreX, centreY, centreZ = 0;

    centreX = (int)CBlobGetXCenter()(*blob);
    centreY = (int)CBlobGetYCenter()(*blob);
    centreZ = (int)depth.at<uchar>(centreX,centreY);


    p.setCentrePos(centreX, centreY, centreZ);

    p.setMaxX(blob->maxx);
    p.setMinX(blob->minx);
    p.setMinY(blob->miny);
    p.setMaxY(blob->maxy);

    int handX, handY, handZ = 0;

    handX = (int)CBlobGetMaxX()(*blob);
    handY = (int)CBlobGetMinYatMaxX()(*blob);
    handZ = (int)depth.at<uchar>(handX,handY);

    p.setHandPos(handX, handY, handZ);


    p.setDistance(calcDistance(blob, depth));
    //p.setPose(findPose(blob));
    //p.setSpeed(calcSpeed(blob));



    return p;

}
/** \brief Finds the pose of blobs
*   \param Pointer to current blob
*   \return Pose value
*
* Method for finding the pose of
* a blob using the ratio between
* max and min values.
*/
int PeopleDetector::findPose(CBlob *blob){
    int pose = 0;
    double min, ratio;

    //Get ratio between "hand" and "head"
    min = CBlobGetMinYatMaxX()(blob);
    ratio = min/blob->miny;

    /* Translate pose to int:
        0: no pose
        1: T -pose, arms raised
    */
    if (ratio < 1.4){
        pose = 1;
    }

    return pose;
}


/** \brief Calculates speed of blobs
*   \param Pointer to current blob
*   \return Speed value
*
* Method for calculating the speed
* of a blob.
*/
float PeopleDetector::calcSpeed(CBlob *blob){
    float speed = 0.0;

    return speed;
}


/** \brief Finds the distance of blobs
*   \param Pointer to current blob, reference to depth image
*   \return Distance value
*
* Method for finding the distance to
* the centre of a blob using a depth image.
*/

float PeopleDetector::calcDistance(CBlob *blob, Mat& depth){
    int x,y;
    int distance;
    x = (int)(blob->maxx - blob->minx)/ 2.0;
    y = (int)(blob->maxy - blob->miny) /2.0;

    distance = (int)depth.at<uchar>(x,y);
    return distance;

}

